#include "ASM330.h"
#include "spi.h"
#include "delay.h"
#include "usart.h"
#include "sys.h"
#include "string.h"


u8 statu1,statu2,statu3,statu4,statu5,statu6;
//spi协议的初始化
void Sensor_SPI_Init(void)
{
	GPIO_InitTypeDef  GPIO_InitStructure;
	//spi2时钟打开
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST , ENABLE);                    //禁止复用
	//片选引脚初始化
  GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3); 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;	
  GPIO_Init(GPIOC, &GPIO_InitStructure);
	GPIO_SetBits(GPIOC, GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3);
	CS_Disable();     //关闭所有ASM330片选
	SPI2_Init();      //初始化spi2 	   	
}


//初始化全部关闭片选
void CS_Disable(void)
{
	ASM330_CS(Disable,CS1);
	ASM330_CS(Disable,CS2);
	ASM330_CS(Disable,CS3);
	ASM330_CS(Disable,CS4);
}


/*
向ASM330寄存器写入数据
reg:寄存器地址
value；写入的值
spix：选择哪个ASM330
*/
void ASM330_Write_Reg(u8 reg,u8 value,u8 SPIx)
{
	if(SPIx==0)
	 {
     ASM330_CS(Enable,CS1);                 //使能SPI传输
     SPI2_ReadWriteByte(reg);               //发送寄存器号 
     SPI2_ReadWriteByte(value);             //写入寄存器的值
     ASM330_CS(Disable,CS1);                //禁止SPI传输
   } 	
  if(SPIx==1)
	 {	
     ASM330_CS(Enable,CS2);                 //使能SPI传输
     SPI2_ReadWriteByte(reg);               //发送寄存器号 
     SPI2_ReadWriteByte(value);             //写入寄存器的值
     ASM330_CS(Disable,CS2);                //禁止SPI传输
   }
  if(SPIx==2)
	 {	
     ASM330_CS(Enable,CS3);                 //使能SPI传输
     SPI2_ReadWriteByte(reg);               //发送寄存器号 
     SPI2_ReadWriteByte(value);             //写入寄存器的值
     ASM330_CS(Disable,CS3);                //禁止SPI传输
   }
  if(SPIx==3)
	 {	
     ASM330_CS(Enable,CS4);                 //使能SPI传输
     SPI2_ReadWriteByte(reg);               //发送寄存器号 
     SPI2_ReadWriteByte(value);             //写入寄存器的值
     ASM330_CS(Disable,CS4);                //禁止SPI传输
   }		
}


/*
读取ASM330寄存器值
reg:要读的寄存器
spix:读取哪个ASM330
*/
u8 ASM330_Read_Reg(u8 reg,u8 SPIx)
{
	u8 reg_val;	
  if(SPIx==0)	
	 {
     ASM330_CS(Enable,CS1);                //使能SPI传输	
     SPI2_ReadWriteByte(reg|0x80);         //发送寄存器号  
     reg_val = SPI2_ReadWriteByte(0XFF);   //读取寄存器内容
     ASM330_CS(Disable,CS1);               //禁止SPI传输	
   }	
  if(SPIx==1)	
	 {
     ASM330_CS(Enable,CS2);                //使能SPI传输		
     SPI2_ReadWriteByte(reg|0x80);         //发送寄存器号  
     reg_val = SPI2_ReadWriteByte(0XFF);   //读取寄存器内容
     ASM330_CS(Disable,CS2);               //禁止SPI传输	
   }		
  if(SPIx==2)	
	 {
     ASM330_CS(Enable,CS3);                //使能SPI传输		
     SPI2_ReadWriteByte(reg|0x80);         //发送寄存器号  
     reg_val = SPI2_ReadWriteByte(0XFF);   //读取寄存器内容
     ASM330_CS(Disable,CS3);               //禁止SPI传输	
   }		
  if(SPIx==3)	
	 {
     ASM330_CS(Enable,CS4);                //使能SPI传输		
     SPI2_ReadWriteByte(reg|0x80);         //发送寄存器号  
     reg_val = SPI2_ReadWriteByte(0XFF);   //读取寄存器内容
     ASM330_CS(Disable,CS4);               //禁止SPI传输	
   }		
  return(reg_val);                         //返回状态值
}	


/*
软件复位和设备配置位为1
deviece_reset:0x05
decice_conf:0xE2
*/
u8 ASM330_RESET_SET(u8 deviece_reset,u8 decice_conf)
{
  u8 ret1,ret2,i;
	for(i=0;i<4;i++)
	{
		ret2=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);                 //读取ASM330的固定值为0x6B，判断能否通信
	  if(ret2==0x6B)
	   {
	     
		      ASM330_Write_Reg(ASM330LHH_CTRL3_C,deviece_reset,i);  //复位ASM330
	        ret1 = ASM330_Read_Reg(ASM330LHH_CTRL9_XL,i);
	     if(ret1!=0XE2)
	      {
		      ASM330_Write_Reg(ASM330LHH_CTRL9_XL,decice_conf,i);
	      }
		 	 switch(i)
		    {
			    case 0: statu1=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,0); break;
			    case 1: statu2=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,1); break;
			    case 2: statu3=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,2); break;
			    case 3: statu4=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,3); break;
		    }
	    }
    }
	 return (ASM330_Read_Reg(ASM330LHH_CTRL9_XL,4));
}

//禁用I2C
u8 ASM330_User_Ctrl(u8 iic_if_dis)
{
	u8 ret,ret1,i;
	for(i=0;i<4;i++)
	 {
		 ret1=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);
		 if(ret1==0x6B)
		  {
	      ret = ASM330_Read_Reg(ASM330LHH_CTRL4_C,i);
        if(ret == 0X00)
	       {
		       ASM330_Write_Reg(ASM330LHH_CTRL4_C,iic_if_dis,i);
	       }
		  }
    }
	 return (ASM330_Read_Reg(ASM330LHH_CTRL4_C,0));
}

/*
设置加计 陀螺采样率 (输出数据速率)和量程
XL_update_set:加计的采样率设置
G_update_set:陀螺仪的采样率设置
XL_fullscale:加计的量程设置
G_fullscale:陀螺仪的量程设置
*/

u8 ASM330_DataUpdate_FullScale_Set(u8 XL_update_set,u8 G_update_set,u8 XL_fullscale,u8 G_fullscale)
{
	u8 ret,ret1,ret2,i;
	for(i=0;i<4;i++)
   { 
	  ret2=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);
	  if(ret2==0x6B)
	   {
	    ret = ASM330_Read_Reg(ASM330LHH_CTRL1_XL,i);
      if(ret == 0x00)
	     { 
		    u8 value1;
		    value1=(XL_update_set|XL_fullscale|0X02);
		    ASM330_Write_Reg(ASM330LHH_CTRL1_XL,value1,i);           //写入ctrl1 配置加计采样率和量程
	     }
      ret1= ASM330_Read_Reg(ASM330LHH_CTRL2_G,i);
	    if(ret1==0X00)
	     { 
		    u8 value2=0;
	      value2=( G_update_set|G_fullscale);
		    ASM330_Write_Reg(ASM330LHH_CTRL2_G,value2,i);            //写入ctrl2 配置陀螺采样率和量程
	     }
     }
    }
   return (ASM330_Read_Reg(ASM330LHH_CTRL1_XL,0));
}
//陀螺仪低通带宽
u8 ASM330_LPF1_bandwidth(u8 Trigger_mode,u8 LPF1_bandwidth)            
{  
	u8 ret=0,ret1,i;
	for(i=0;i<4;i++)
	{
	  ret1=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);
	  if(ret1==0x6B)
	   {
	    ret = ASM330_Read_Reg(ASM330LHH_CTRL6_G,i);
	    if(ret==0X00)
	     {  
		    u8 value=0;
		    value =Trigger_mode|LPF1_bandwidth;
		    ASM330_Write_Reg(ASM330LHH_CTRL6_G,value,i);
	     }
     }
  }
 return (ASM330_Read_Reg(ret,0));
}
 //加速度高通带宽   
u8  ASM330_LPF2_path_set(u8 lpf2_pass_bandwidth)                  
{  
	u8 ret,ret1,i;
	for(i=0;i<4;i++)
	 {
		ret1=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);
		if(ret1==0x6B)
		 {
	    ret=ASM330_Read_Reg(ASM330LHH_CTRL8_XL,i);
	    if(ret==0X00)
	     {
		     ASM330_Write_Reg(ASM330LHH_CTRL8_XL,lpf2_pass_bandwidth,i);
	     }
	   }
    }
	 return (ASM330_Read_Reg(ASM330LHH_CTRL8_XL,0));
}

u8 ASM330_Read_Buf(u8 reg,u8 *pBuf,u8 len,u8 SPIx)                   //读取ASM330缓冲区的数据
{
  u8 status=0, ctr=0;	
  if(SPIx==0x01)	
	 {
    ASM330_CS(Enable,CS1);                                           //使能SPI1传输
    status = SPI2_ReadWriteByte(reg|0x80);                           //发送寄存器值(位置),并读取状态值   	   
    for(ctr=0;ctr<len;ctr++) 
		  pBuf[ctr]=SPI2_ReadWriteByte(0XFF);                            //读出数据
    ASM330_CS(Disable,CS1);                                          //关闭SPI传输
	 }
	if(SPIx==0x02)	
	 {
    ASM330_CS(Enable,CS2);                                            //使能SPI2传输
    status = SPI2_ReadWriteByte(reg|0x80);                            //发送寄存器值(位置),并读取状态值   	   
    for(ctr=0;ctr<len;ctr++) 
		  pBuf[ctr]=SPI2_ReadWriteByte(0XFF);                             //读出数据
    ASM330_CS(Disable,CS2);                                           //关闭SPI传输

		
	}
	if(SPIx==0x03)	
	 {
    ASM330_CS(Enable,CS3);                                            //使能SPI3传输
    status = SPI2_ReadWriteByte(reg|0x80);                            //发送寄存器值(位置),并读取状态值   	   
    for(ctr=0;ctr<len;ctr++)
		  pBuf[ctr]=SPI2_ReadWriteByte(0XFF);       								      //读出数据
    ASM330_CS(Disable,CS3);                                           //关闭SPI传输
	 }
	if(SPIx==0x04)	
	 {
    ASM330_CS(Enable,CS4);                                            //使能SPI4传输
    status = SPI2_ReadWriteByte(reg|0x80);                            //发送寄存器值(位置),并读取状态值   	   
    for(ctr=0;ctr<len;ctr++)
		  pBuf[ctr]=SPI2_ReadWriteByte(0XFF);                             //读出数据
    ASM330_CS(Disable,CS4);                                           //关闭SPI传输
	 }
  return status;                                                      //返回读到的状态值
}
//片选使能
void ASM330_CS(u8 flag,u8 CSx)  
{ 
	if(CSx==1)
   {
	  ASM330_CS_ACC_1=Isenable(flag);
   }
  if(CSx==2)
   {
	  ASM330_CS_ACC_2=Isenable(flag);
   }
  if(CSx==3)
   {
	  ASM330_CS_ACC_3=Isenable(flag);
   }
  if(CSx==4)
   {
	  ASM330_CS_ACC_4=Isenable(flag);
   }
}


u8 Isenable(u8 flag)
{
	if(flag==1)
	   {
		  return 0;
	   }
	if(flag==0)
	   {
		  return 1;
	   }
	return 1;
}





u8  INT2_Update(u8 data_ready)
{
	u8 ret,ret1,i;
	for(i=0;i<4;i++)
   {
	  ret1=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);
	  if(ret1==0x6B)
	   {
	    ret=ASM330_Read_Reg( ASM330LHH_INT2_CTRL,i);
	    if(ret==0X00)
	     {
		     ASM330_Write_Reg(ASM330LHH_INT2_CTRL,data_ready,i);
	     }
     }
   }
	return (ASM330_Read_Reg(ASM330LHH_INT2_CTRL,0));
}
u8 Dataready_Pulsed(u8 dataready_pulsed)
{
	u8 ret,ret1,i;
	for(i=0;i<4;i++)
   {
	  ret1=ASM330_Read_Reg(ASM330LHH_WHO_AM_I,i);
	  if(ret1==0x6B)
	   {
	    ret=ASM330_Read_Reg( ASM330LHH_COUNTER_BDR_REG1,i);
	    if(ret==0X00)
	     {
		    ASM330_Write_Reg(ASM330LHH_COUNTER_BDR_REG1,dataready_pulsed,i);
	     }
     }
   }
	delay_ms(1000);
	return (ASM330_Read_Reg(ASM330LHH_COUNTER_BDR_REG1,0));
}
//第一个ASM330转化成报文数据格式
void ASM330_ChangeData_SPI1(u8 *aBuf,u8 *bBuf,u8 IMU)           
{   
	if(IMU==0)
	 {
	  bBuf[0]=0XAA;
		bBuf[1]=0XB1;
		bBuf[2]=aBuf[9];                                            // Acc X H8              
		bBuf[3]=aBuf[8];                                            // Acc X L8           
	  bBuf[4]=aBuf[11];                                           // Acc Y H8               
	  bBuf[5]=aBuf[10];                                           // Acc Y L8                 
		bBuf[6]=aBuf[13];                                           // Acc Z H8                   
		bBuf[7]=aBuf[12];                                           // Acc Z L8                          
		bBuf[8]=aBuf[3];                                            //Gyro X H8               
		bBuf[9]=aBuf[2];                                            //Gyro X L8                  
		bBuf[10]=aBuf[5];                                           //Gyro Y H8               
		bBuf[11]=aBuf[4];                                           //Gyro Y L8             
		bBuf[12]=aBuf[7];                                           //Gyro Z H8         
		bBuf[13]=aBuf[6];                                           //Gyro Z L8       
		bBuf[14]=aBuf[1];                                           //  Temp H8 
		bBuf[15]=aBuf[0];                                           //  Temp L8
		bBuf[16]=bBuf[0]+bBuf[1]+bBuf[2]+bBuf[3]+bBuf[4]+bBuf[5]+bBuf[6]+bBuf[7]+bBuf[8]+bBuf[9]+bBuf[10]+bBuf[11]+bBuf[12]+bBuf[13]+bBuf[14]+bBuf[15];
	  bBuf[16]=(bBuf[16]&0x00FF);
	 }
	
}
//第二个ASM330转化成报文数据格式
void ASM330_ChangeData_SPI2(u8 *aBuf,u8 *bBuf,u8 IMU)
{
	if(IMU==0)
	 {
	  bBuf[17]=0XAA;
		bBuf[18]=0XB2;
		bBuf[19]=aBuf[9];                                            // Acc X H8              
		bBuf[20]=aBuf[8];                                            // Acc X L8           
	  bBuf[21]=aBuf[11];                                           // Acc Y H8               
	  bBuf[22]=aBuf[10];                                           // Acc Y L8                 
		bBuf[23]=aBuf[13];                                           // Acc Z H8                   
		bBuf[24]=aBuf[12];                                           // Acc Z L8                          
		bBuf[25]=aBuf[3];                                            //Gyro X H8               
		bBuf[26]=aBuf[2];                                            //Gyro X L8                  
		bBuf[27]=aBuf[5];                                            //Gyro Y H8               
		bBuf[28]=aBuf[4];                                            //Gyro Y L8             
		bBuf[29]=aBuf[7];                                            //Gyro Z H8         
		bBuf[30]=aBuf[6];                                            //Gyro Z L8       
		bBuf[31]=aBuf[1];                                            //  Temp H8 
		bBuf[32]=aBuf[0];                                            //  Temp L8
		bBuf[33]=bBuf[17]+bBuf[18]+bBuf[19]+bBuf[20]+bBuf[21]+bBuf[22]+bBuf[23]+bBuf[24]+bBuf[25]+bBuf[26]+bBuf[27]+bBuf[28]+bBuf[29]+bBuf[30]+bBuf[31]+bBuf[32];
	  bBuf[33]=(bBuf[33]&0x00FF);
	 }
		
}
//第三个ASM330转化成报文数据格式
void ASM330_ChangeData_SPI3(u8 *aBuf,u8 *bBuf,u8 IMU)
{
	if(IMU==0)
 	 {
	  bBuf[34]=0XAA;
		bBuf[35]=0XB3;
		bBuf[36]=aBuf[9];                                            // Acc X H8              
		bBuf[37]=aBuf[8];                                            // Acc X L8           
	  bBuf[38]=aBuf[11];                                           // Acc Y H8               
	  bBuf[39]=aBuf[10];                                           // Acc Y L8                 
		bBuf[40]=aBuf[13];                                           // Acc Z H8                   
		bBuf[41]=aBuf[12];                                           // Acc Z L8                          
		bBuf[42]=aBuf[3];                                            //Gyro X H8               
		bBuf[43]=aBuf[2];                                            //Gyro X L8                  
		bBuf[44]=aBuf[5];                                            //Gyro Y H8               
		bBuf[45]=aBuf[4];                                            //Gyro Y L8             
		bBuf[46]=aBuf[7];                                            //Gyro Z H8         
		bBuf[47]=aBuf[6];                                            //Gyro Z L8       
		bBuf[48]=aBuf[1];                                            //  Temp H8 
		bBuf[49]=aBuf[0];                                            //  Temp L8
		bBuf[50]=bBuf[34]+bBuf[35]+bBuf[36]+bBuf[37]+bBuf[38]+bBuf[39]+bBuf[40]+bBuf[41]+bBuf[42]+bBuf[43]+bBuf[44]+bBuf[45]+bBuf[46]+bBuf[47]+bBuf[48]+bBuf[49];
	  bBuf[50]=(bBuf[50]&0x00FF);
 	 }
}
//第四个ASM330转化成报文数据格式
void ASM330_ChangeData_SPI4(u8 *aBuf,u8 *bBuf,u8 IMU)
{
	if(IMU==0)
 	 {
	  bBuf[51]=0XAA;
		bBuf[52]=0XB4;
		bBuf[53]=aBuf[9];                                            // Acc X H8              
		bBuf[54]=aBuf[8];                                            // Acc X L8           
	  bBuf[55]=aBuf[11];                                           // Acc Y H8               
	  bBuf[56]=aBuf[10];                                           // Acc Y L8                 
		bBuf[57]=aBuf[13];                                           // Acc Z H8                   
		bBuf[58]=aBuf[12];                                           // Acc Z L8                          
		bBuf[59]=aBuf[3];                                            //Gyro X H8               
		bBuf[60]=aBuf[2];                                            //Gyro X L8                  
		bBuf[61]=aBuf[5];                                            //Gyro Y H8               
		bBuf[62]=aBuf[4];                                            //Gyro Y L8             
		bBuf[63]=aBuf[7];                                            //Gyro Z H8         
		bBuf[64]=aBuf[6];                                            //Gyro Z L8       
		bBuf[65]=aBuf[1];                                            //  Temp H8 
		bBuf[66]=aBuf[0];                                            //  Temp L8
		bBuf[67]=bBuf[51]+bBuf[52]+bBuf[53]+bBuf[54]+bBuf[55]+bBuf[56]+bBuf[57]+bBuf[58]+bBuf[59]+bBuf[60]+bBuf[61]+bBuf[62]+bBuf[63]+bBuf[64]+bBuf[65]+bBuf[66];
	  bBuf[67]=(bBuf[67]&0x00FF);
 	 }
}







